#include "APZP1Algorithm.h"
#include "../RSTController.h"
#include "Utils.h"

#include <sstream>
#include <numeric>
#include <cmath>

namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace SimInterface;

const double APZP1Algorithm::DEFAULT_LAMBDA = 0.99;

const string APZP1Algorithm::CONF_AMPOLY("ampoly");
const string APZP1Algorithm::CONF_BMPOLY("bmpoly");
//const string APZP1Algorithm::CONF_BPLUS("bplus");
const string APZP1Algorithm::CONF_RDEGREE("ndr");
const string APZP1Algorithm::CONF_SDEGREE("nds");
const string APZP1Algorithm::CONF_KDELAY("kdly");

// Do debugowania:
//static void printVect1(ostream& o, const std::string n, const RmnkSolver::Matrix& m)
//{
//    o << n << ":[ ";
//    RmnkSolver::Matrix::const_iterator1 it;
//    for(it =m.begin1(); it != m.end1(); ++it)
//        o << *it << ' ';
//    o << "]" << endl;
//}

APZP1Algorithm::APZP1Algorithm(RSTController& owner, int k, int ndR, int ndS )
    : AdaptiveAlgorithmBase(owner, "apzp1", "APZP1"),
      m_k(k), m_ndR(ndR), m_ndS(ndS)
{
    reset();
}

void APZP1Algorithm::adapt(double ctrlVar, double processVar, double setPoint)
{
    (void) setPoint; // niewykorzystane

    // Funkcja będzie wyznaczać wielomiany R i S na podstawie równania predykcyjnego
    // gamma(i) = R*u(i-k) + S*y(i-k)
    // gdzie gamma(i) = Am*y(i)

    double gamma;

    // Najpierw wkładamy próbki y(i) do pamięci gdyż potrzebujemy wartości bierzących.
    // processVar = y(i)
    // ctrlVar = u(i)
    m_ymem.push_front(processVar);
    m_ymem.pop_back();
    m_umem.push_front(ctrlVar);
    m_umem.pop_back();

    // Na chwilą obecną:
    // m_ymem zawiera y(i) .. y(i-k-dS) lub y(i-dAm) - zależy, co ma większy rząd
    // m_umem zawiera u(i) .. u(i-k-dR)

    // Definicja iteratorów:
    vector<double>::iterator
            ampoly_it = m_AmPoly.begin(),                    // zaczynamy od am0
            ampoly_last_it = m_AmPoly.end();
    deque<double>::iterator
            ymem_it = m_ymem.begin();                        // zaczynamy od y(i)

    // gamma(i) = Am0*y(i) + Am1*y(i-1) + ... + AmdAm*y(i-dAm)
    gamma = inner_product(ampoly_it, ampoly_last_it, ymem_it, double(0), plus<double>(), multiplies<double>());

    // Budujemy wektor próbek dla identyfikacji

    copy(m_umem.begin() + m_k, m_umem.end(), m_phi.begin1());
    copy(m_ymem.begin() + m_k, m_ymem.begin() + m_k + m_ndS + 1, m_phi.begin1() + m_ndR + 1);

    //printVect1(cout, "theta(i) ", m_theta);
    //printVect1(cout, "phi(i) ", m_phi);

    // Identyfikuj i wymuś r0 = 1
    m_theta = m_wrmnk->identify(m_theta, m_phi, gamma);

    // m_theta(0, 0) przechowuje parametr r0. Obiekt identyfikacji nie wie że
    // wielomian R powinien być moniczny, więc musimy sami zadbać o skrócenie wielomianów
    // przez r0. Nie wolno skracać bezpośrednio m_theta, gdyż ingerujemy w ten
    // sposób w działanie identyfikacji (tracimy zgodność oceny parametrów).
    double reduceRatio = m_theta(0, 0);

    // Konwersja theta na wielomiany R i S.
    copy(m_theta.begin1(), m_theta.begin1() + m_ndR + 1, m_r.begin());
    copy(m_theta.begin1() + m_ndR+1, m_theta.end1(), m_s.begin());

    // Dopiero teraz skracamy. Theta pozostaje bez zmian.
    m_r /= reduceRatio;
    m_s /= reduceRatio;
    m_t = m_BmPoly / reduceRatio;


    // FINITO
}

void APZP1Algorithm::reset()
{
    resetWrnmnk();
    resizeBuffers();
    cleanBuffers();

    m_r = Polynomial(m_ndR, 0);
    m_s = Polynomial(m_ndS, 0);
    m_r[0] = 1;
}

void APZP1Algorithm::resetWrnmnk()
{
    // A := R,
    // B := S

    unsigned order = m_ndR + m_ndS + 2;

    m_wrmnk.reset(new RmnkSolver(order, DEFAULT_LAMBDA));

    // theta = [r0 .. rdR s0 .. sdS]
    m_theta = RmnkSolver::Matrix(order, 1);

    fill(m_theta.begin1(), m_theta.end1(), 0);
    m_theta(0, 0) = 1;

    // phi = [u(i-k) .. u(i-k-dR) y(i-k) .. y(i-k-dS)]
    m_phi = RmnkSolver::Matrix(order, 1);
    fill(m_phi.begin1(), m_phi.end1(), 0);
}

void APZP1Algorithm::resizeBuffers()
{
    // Pamięć potrzebna na próbki od y(i) do y(i-k-dS) lub y(i-dAm)
    int degree = max(m_ndS + m_k + 1, m_AmPoly.degree() + 1);
    m_ymem.resize(degree);

    // Pamięć potrzebna na próbki od u(i) do u(i-k-dR)
    m_umem.resize(m_ndR + m_k + 1);

}

void APZP1Algorithm::cleanBuffers()
{
    fill(m_ymem.begin(), m_ymem.end(), 0);
    fill(m_umem.begin(), m_umem.end(), 0);
}

//
// Konfiguracja parametrów z poziomu GIU
//

void APZP1Algorithm::setAMPoly(const Polynomial& amPoly)
{
    m_AmPoly = amPoly;
    resizeBuffers();
}

void APZP1Algorithm::setBMPoly(const Polynomial& bmPoly)
{
    m_t = m_BmPoly = bmPoly;
    getOwner().resizeBuffersIfNecessary();
}

//void APZP1Algorithm::setBPlusPoly(const Polynomial& bpPoly)
//{
//    m_BPlusPoly = bpPoly;
//}

void APZP1Algorithm::setRDegree(unsigned int ndR)
{
    m_ndR = ndR;
    reset();
    getOwner().resizeBuffersIfNecessary();
}


void APZP1Algorithm::setSDegree(unsigned int ndS)
{
    m_ndS = ndS;
    reset();
    getOwner().resizeBuffersIfNecessary();
}


void APZP1Algorithm::setKDelay(unsigned int k)
{
    m_k = k;
    reset();
    getOwner().resizeBuffersIfNecessary();
}


bool APZP1Algorithm::setParameter(const string& param, const string& value) throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string tvalue = Utils::trimWhiteSpaces(value);

    // Strumień danych dla łańcucha argumentu.
    istringstream istr(tvalue);

    double tmpVal;
    istr >> tmpVal;

    bool success = true;

    if(tparam == CONF_AMPOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            setAMPoly(poly);
        }
    }
    else if(tparam == CONF_BMPOLY)
    {
        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
        if(success)
        {
            setBMPoly(poly);
        }
    }
//    else if(tparam == CONF_BPLUS)
//    {
//        Polynomial poly = Polynomial(Utils::convertStrToVector(tvalue, /* out */ success));
//        if(success)
//        {
//            setBPlusPoly(poly);
//        }
//    }
    else if(tparam == CONF_RDEGREE)
    {
        if(!istr.bad())
        {
            setRDegree(tmpVal);
        }
    }
    else if(tparam == CONF_SDEGREE)
    {
        if(!istr.bad())
        {
            setSDegree(tmpVal);
        }
    }
    else if(tparam == CONF_KDELAY)
    {
        if(!istr.bad())
        {
            setKDelay(tmpVal);
        }
    }
    else
        throw iconf_error::unrecognizedParam(*this, tparam);

    if(istr.bad() || !success)
        throw iconf_error::syntaxError(*this, tvalue);

    return true;
}

std::string APZP1Algorithm::getParameter(const std::string& param) const throw(std::logic_error)
{
    // Usuń ewentualne wiodące i końcowe spacje
    string tparam = Utils::trimWhiteSpaces(param);
    string outStr;

    // Strumień danych dla łańcucha argumentu.
    ostringstream ostr(outStr);

    if(tparam == CONF_AMPOLY)
    {
        ostr << m_AmPoly;
    }
    else if(tparam == CONF_BMPOLY)
    {
        ostr << m_t;
    }
//    else if(tparam == CONF_BPLUS)
//    {
//        ostr << m_BPlusPoly;
//    }
    else if(tparam == CONF_RDEGREE)
    {
        ostr << m_ndR;
    }
    else if(tparam == CONF_SDEGREE)
    {
        ostr << m_ndS;
    }
    else if(tparam == CONF_KDELAY)
    {
        ostr << m_k;
    }
    else
        return AdaptiveAlgorithmBase::getParameter(tparam);

    return ostr.str();
}

SimFile::ConfigWriter& APZP1Algorithm::dumpParameters(SimFile::ConfigWriter& o) const
{
    o.pushParam("adpt::" + CONF_AMPOLY, m_AmPoly);
//    o.pushParam("adpt::" + CONF_BPLUS, m_BPlusPoly);
    o.pushParam("adpt::" + CONF_BMPOLY, m_t);
    o.pushParam("adpt::" + CONF_RDEGREE, m_ndR);
    o.pushParam("adpt::" + CONF_SDEGREE, m_ndS);
    o.pushParam("adpt::" + CONF_KDELAY, m_k);
    return o;
}

}
}
